#include "usr_inc.h"

/* 定义底盘加速度限制 */
#define CHASSIS_ACC_LIMIT 1.0f

/* 定义底盘横向加速度限制 */
#define CHASSIS_LAT_ACC_LIMIT 0.275f

/* 触发反向缓冲的最小夹角, 单位为度 */
#define REVERSE_BRAKE_ANGLE 120.0f

/* 定义底盘刹车加速度 */
#define CHASSIS_ACC_BRAKE 0.4f

/* 定义底盘速度最小量 */
#define CHASSIS_EPS_SPD 0.5f

/* 定义底盘模式 */
ChassisMode_t chassisMode = YAW_MODE;

/* 定义定位仪实例 */
Locater_t loc = {0};

/* 定义摇杆最大速度 */
float joyMaxVel = 3.15;  // 单位为m/s
float joyMaxOmega = 4.5; // 单位为rad/s

/* 定义走点pid实例 */
Pid_t pointPid;

/* 定义角度环pid实例 */
Pid_t yawVelPid;
Pid_t yawPosPid;

/* 定义卡尔曼滤波输出值 */
float MergeX = 0.0;
float MergeY = 0.0;

/* 定义yaw角偏差值 */
float yawBias = 0.0;

/**
 * @brief 初始化所有底盘相关的实例
 */
void ALL_CHASSIS_Init(void)
{
    PidInit_t pidInit = {0};

    pidInit.kp = 3.8;
    pidInit.ki = 0.0;
    pidInit.kd = 180.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 3.0 / 3;
    pidInit.maxOutput = 3.0;
    pidInit.minOutput = -3.0;
    PID_Init(&pointPid, &pidInit);

    pidInit.kp = 1.75;
    pidInit.ki = 0.00025;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 4.5 / 3;
    pidInit.maxOutput = 4.5;
    pidInit.minOutput = -4.5;
    PID_Init(&yawVelPid, &pidInit);

    pidInit.kp = 0.1;
    pidInit.ki = 0.0;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 4.5 / 3;
    pidInit.maxOutput = 4.5;
    pidInit.minOutput = -4.5;
    PID_Init(&yawPosPid, &pidInit);
}

void StartJoyExe(void *argument)
{
    for (;;)
    {
        VelMsg_t velMsg = {0};
        JoyStickMsg_t joyMsg = {0};
        float joyX = 0.0, joyY = 0.0, joyO = 0.0;

        if (xQueueReceive(joyMsgQueueHandle, &joyMsg, 250) == pdTRUE)
        {
            /* 摇杆介入 */
            if (chassisMode == POINT_MODE)
            {
                if (abs(joyMsg.leftX) - 200 >= 0 || abs(joyMsg.leftY) - 200 >= 0)
                {
                    chassisMode = YAW_MODE;
                }
            }

            /* 线性映射 */
            joyX = ALGO_LinearMap(joyMsg.leftX, -JOY_MAX, JOY_MAX, -joyMaxVel, joyMaxVel);
            joyY = ALGO_LinearMap(joyMsg.leftY, -JOY_MAX, JOY_MAX, -joyMaxVel, joyMaxVel);
            joyO = ALGO_LinearMap(joyMsg.rightY, -JOY_MAX, JOY_MAX, -joyMaxOmega, joyMaxOmega);
            if (hypotf(joyX, joyY) > joyMaxVel)
            {
                float theta = atan2f(joyY, joyX);
                joyX = joyMaxVel * cosf(theta);
                joyY = joyMaxVel * sinf(theta);
            }
        }

        /* 限制加速度 */
        VelMsg_t vMsg;
        xQueuePeek(chassisActVelBoxHandle, &vMsg, portMAX_DELAY);
        float vMag = hypotf(vMsg.velX, vMsg.velY);
        float dvX = joyX - vMsg.velX;
        float dvY = joyY - vMsg.velY;
        float dvMag = hypotf(dvX, dvY);

        if (vMag < CHASSIS_EPS_SPD) // 如果当前速度非常小，退回到整体限幅逻辑
        {
            if (dvMag > CHASSIS_ACC_LIMIT)
            {
                float theta = atan2f(dvY, dvX);
                joyX = vMsg.velX + CHASSIS_ACC_LIMIT * cosf(theta);
                joyY = vMsg.velY + CHASSIS_ACC_LIMIT * sinf(theta);
            }
        }
        else
        {
            float joyMag = hypotf(joyX, joyY);

            float vDir = atan2f(vMsg.velY, vMsg.velX);
            float joyDir = atan2f(joyY, joyX);
            float dDir = fabsf(joyDir - vDir);
            if (dDir > M_PI) dDir = 2 * M_PI - dDir;
            dDir = dDir * 180.0f / M_PI;

            if (dDir > REVERSE_BRAKE_ANGLE && vMag > CHASSIS_EPS_SPD && joyMag > CHASSIS_EPS_SPD) // 反向缓冲
            {
                float decMag = CHASSIS_ACC_LIMIT;
                if (joyMag < CHASSIS_ACC_LIMIT)
                {
                    decMag = joyMag; // 避免过减
                }
                joyX = vMsg.velX - decMag * cosf(vDir);
                joyY = vMsg.velY - decMag * sinf(vDir);
            }
            else
            {
                /* 构造前向单位向量 */
                float uX = vMsg.velX / vMag;
                float uY = vMsg.velY / vMag;

                /* 构造右手系下的横向单位向量 */
                float lX = -uY;
                float lY = uX;

                /* 投影出前向和横向的速度变化 */
                float dvFWD = dvX * uX + dvY * uY;
                float dvLAT = dvX * lX + dvY * lY;

                /* 限制横向变化 */
                if (fabsf(dvLAT) > CHASSIS_LAT_ACC_LIMIT)
                {
                    dvLAT = copysignf(CHASSIS_LAT_ACC_LIMIT, dvLAT);
                }

                /* 先构建新的dv向量，再做总量限幅 */
                dvX = dvFWD * uX + dvLAT * lX;
                dvY = dvFWD * uY + dvLAT * lY;
                dvMag = hypotf(dvX, dvY);
                if (dvMag > CHASSIS_ACC_LIMIT)
                {
                    float scale = CHASSIS_ACC_LIMIT / dvMag;
                    dvX *= scale;
                    dvY *= scale;
                }

                /* 应用限幅后的目标速度 */
                joyX = vMsg.velX + dvX;
                joyY = vMsg.velY + dvY;
            }
        }

        /* 处理急刹车 */
        if (fabsf(joyX) <= 0.001)
        {
            if (fabsf(vMsg.velX) >= CHASSIS_ACC_BRAKE)
            {
                joyX = vMsg.velX - copysignf(CHASSIS_ACC_BRAKE, vMsg.velX);
            }
            else
            {
                joyX = 0.0;
            }
        }
        if (fabsf(joyY) <= 0.001)
        {
            if (fabsf(vMsg.velY) >= CHASSIS_ACC_BRAKE)
            {
                joyY = vMsg.velY - copysignf(CHASSIS_ACC_BRAKE, vMsg.velY);
            }
            else
            {
                joyY = 0.0;
            }
        }

        /* 转化为世界坐标系 */
        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
        CHASSIS_MergeYaw(&locMsg, yawBias);
        velMsg.velX = joyX * cosf(locMsg.posYaw / 180.0 * M_PI) + joyY * sinf(locMsg.posYaw / 180.0 * M_PI);
        velMsg.velY = (-1) * joyX * sinf(locMsg.posYaw / 180.0 * M_PI) + joyY * cosf(locMsg.posYaw / 180.0 * M_PI);
        velMsg.omega = joyO;

        xQueueOverwrite(chassisJoyVelMsgBoxHandle, &velMsg);
    }
}

void StartChassisPointExe(void *argument)
{
    PID_SetTarget(&pointPid, 0);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        VelMsg_t velMsg = {0};

        SCurveProcess(&sCurve0);
        SCurveProcess(&sCurve1);

        if (chassisMode == POINT_MODE)
        {
            LocaterMsg_t locMsg;
            xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
            CHASSIS_MergeYaw(&locMsg, yawBias);

            float errX = sCurve0.out - locMsg.posX;
            float errY = sCurve1.out - locMsg.posY;
            float distance = -hypotf(errX, errY);
            PID_Calculate(&pointPid, distance);

            if (fabs(distance) <= 0.01)
            {
                velMsg.velX = 0.0;
                velMsg.velY = 0.0;
            }
            else
            {
                float v = PID_GetOutput(&pointPid);
                float rawX = v * cosf(atan2f(errY, errX));
                float rawY = v * sinf(atan2f(errY, errX));
                velMsg.velX = rawX * cosf(locMsg.posYaw / 180.0 * M_PI) + rawY * sinf(locMsg.posYaw / 180.0 * M_PI);
                velMsg.velY = (-1) * rawX * sinf(locMsg.posYaw / 180.0 * M_PI) + rawY * cosf(locMsg.posYaw / 180.0 * M_PI);
            }
        }
        xQueueOverwrite(chassisPointVelMsgBoxHandle, &velMsg);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisYawExe(void *argument)
{
    const float posDeadzone = 0.2;

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        VelMsg_t velMsg = {0};

        if (chassisMode == YAW_MODE || chassisMode == POINT_MODE)
        {
            LocaterMsg_t locMsg;
            xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
            CHASSIS_MergeYaw(&locMsg, yawBias);

            const float posErr = fabs(locMsg.posYaw - PID_GetTarget(&yawPosPid));

            PID_Calculate(&yawPosPid, locMsg.posYaw);
            if (posErr <= posDeadzone)
            {
                PID_SetTarget(&yawVelPid, 0.0);
            }
            else
            {
                PID_SetTarget(&yawVelPid, PID_GetOutput(&yawPosPid));
            }

            PID_Calculate(&yawVelPid, locMsg.velYaw);
            if (posErr <= posDeadzone)
            {
                velMsg.omega = 0.0;
            }
            else
            {
                velMsg.omega = PID_GetOutput(&yawVelPid);
            }
        }
        xQueueOverwrite(chassisYawVelMsgBoxHandle, &velMsg);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisSendVel(void *argument)
{
    float velX = 0.0;
    float velY = 0.0;
    float omega = 0.0;

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        VelMsg_t joyVel;
        xQueuePeek(chassisJoyVelMsgBoxHandle, &joyVel, portMAX_DELAY);

        VelMsg_t pointVel;
        xQueuePeek(chassisPointVelMsgBoxHandle, &pointVel, portMAX_DELAY);

        VelMsg_t yawVel;
        xQueuePeek(chassisYawVelMsgBoxHandle, &yawVel, portMAX_DELAY);

        velX = joyVel.velX + pointVel.velX;
        velY = joyVel.velY + pointVel.velY;
        omega = joyVel.omega + yawVel.omega;

        uint8_t buf[16] = {0};
        memcpy(buf + 0, &velX, sizeof(float));
        memcpy(buf + 4, &velY, sizeof(float));
        memcpy(buf + 8, &omega, sizeof(float));
        BSP_FDCAN_SendData(&hfdcan2, 0x100, buf, FDCAN_DLC_BYTES_16);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisCmdDeal(void *argument)
{
    for (;;)
    {
        ChassisCmdMsg_t cmd;
        PointMsg_t pointMsg;
        float yawMsg;

        if (xQueueReceive(chassisCmdQueueHandle, &cmd, 2) == pdTRUE)
        {
            if (cmd == CHASSIS_FREE)
            {
                BSP_FDCAN_SendData(&hfdcan2, 0x160, NULL, FDCAN_DLC_BYTES_0);
            }
            else if (cmd == CHASSIS_LOCK)
            {
                BSP_FDCAN_SendData(&hfdcan2, 0x161, NULL, FDCAN_DLC_BYTES_0);
            }
            else if (cmd == CHASSIS_CONF_YAW)
            {
                chassisMode = YAW_MODE;
            }
            else if (cmd == CHASSIS_CONF_JOY)
            {
                chassisMode = JOY_MODE;
            }
            else if (cmd == CHASSIS_CONF_POINT)
            {
                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                SCurveHardProcess(&sCurve0, locMsg.posX);
                SCurveHardProcess(&sCurve1, locMsg.posY);
                chassisMode = POINT_MODE;
            }
            else if (cmd == CHASSIS_LOW_SPD)
            {
                joyMaxVel = 0.5;
                joyMaxOmega = 1.5;
            }
            else if (cmd == CHASSIS_MID_SPD)
            {
                joyMaxVel = 2.0;
                joyMaxOmega = 4.5;
            }
            else if (cmd == CHASSIS_HIGH_SPD)
            {
                joyMaxVel = 3.15;
                joyMaxOmega = 4.5;
            }
        }

        if (xQueueReceive(chassisPointCmdQueueHandle, &pointMsg, 2) == pdTRUE)
        {
            sCurve0.speedTar = pointMsg.posX;
            sCurve1.speedTar = pointMsg.posY;
        }

        if (xQueueReceive(chassisYawCmdQueueHandle, &yawMsg, 2) == pdTRUE)
        {
            PID_SetTarget(&yawPosPid, yawMsg);
        }
    }
}

void StartLocMerge(void *argument)
{
    KFP KfpLocX = {0.02, 0, 0, 0, 0.001, 0.543};
    KFP KfpLocY = {0.02, 0, 0, 0, 0.001, 0.543};
    KFP KfpRadarX = {0.02, 0, 0, 0, 0.001, 0.543};
    KFP KfpRadarY = {0.02, 0, 0, 0, 0.001, 0.543};

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        LocaterMsg_t radarMsg;
        xQueuePeek(radarLocHandle, &radarMsg, portMAX_DELAY);
        RADAR_ConvertToCenter(&radarMsg);

        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

        LocaterMsg_t Merge_locMsg;
        MergeX = kalmanFilter_Merge(&KfpLocX, &KfpRadarX, locMsg.posX, radarMsg.posX);
        MergeY = kalmanFilter_Merge(&KfpLocY, &KfpRadarY, locMsg.posY, radarMsg.posY);

        // printf("%f,%f,%f,%f,%f,%f\r\n", Radar_locMsg.posX,Mapan_locMsg.posX,Merge_Loc_X,Radar_locMsg.posY,Mapan_locMsg.posY,Merge_Loc_Y);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisCalVel(void *argument)
{
    LocaterMsg_t lastLocMsg = {0};

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 20; ;)
    {
        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

        VelMsg_t act;
        act.velX = (locMsg.posX - lastLocMsg.posX) / (0.001 * period);
        act.velY = (locMsg.posY - lastLocMsg.posY) / (0.001 * period);
        xQueueOverwrite(chassisActVelBoxHandle, &act);

        lastLocMsg = locMsg;

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisGetBias(void *argument)
{
    /* 校正速率 */
    const float biasAlpha = 0.5f;

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 1000 / RADAR_FPS + 1; ;)
    {
        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

        LocaterMsg_t radarMsg;
        xQueuePeek(radarLocHandle, &radarMsg, portMAX_DELAY);

        float error = ALGO_Rad2Deg(radarMsg.posYaw) - (locMsg.posYaw + yawBias);
        yawBias += biasAlpha * error;

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisPrintf(void *argument)
{
    vTaskSuspend(NULL);

    for (const int16_t period = 30; ;)
    {
        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

        LocaterMsg_t radarMsg;
        // xQueuePeek(radarLocHandle, &radarMsg, portMAX_DELAY);

        printf("%f, %f, %f, %f, %f, %d, %d\r\n", locMsg.posX, locMsg.posY, locMsg.posYaw, ALGO_Rad2Deg(radarMsg.posYaw), locMsg.posYaw + yawBias, huart4.hdmarx->State, huart4.hdmarx->ErrorCode);

        vTaskDelay(pdMS_TO_TICKS(period));
    }
}

/**
 * @brief 融合yaw角数据
 */
void CHASSIS_MergeYaw(LocaterMsg_t *locMsg, const float yawBias)
{
    locMsg->posYaw = locMsg->posYaw + yawBias;
}
